#ifndef ANALYSISRESULT_HPP
#define ANALYSISRESULT_HPP

#include <opencv2/opencv.hpp>
#include <sstream>

class AnalysisResult
{
public:
    cv::Mat bwDigit[2];
    cv::Rect roiDigit[2];
    double property;
    cv::Vec3d location;
public:
    AnalysisResult()
    {

    }
    //add path finding---------------------------------------------------
    void pathFinding(int curX, int curY, int endX, int endY)
    {
        Dstar *dstar = new Dstar();
        list<state> mypath;

        dstar->init(curX,curY,endX,endY);         // set start to (0,0) and goal to (10,5)
        //dstar->init(curX,curY,endX,endY);
        //dstar->updateCell(1,1,-1);     // set cell (3,4) to be non traversable
        //dstar->updateCell(1,0,-1);
        //dstar->updateCell(1,-1,-1);
        //dstar->updateCell(2,2,42.432); // set set (2,2) to have cost 42.432

//        dstar->replan();               // plan a path
//        mypath = dstar->getPath();     // retrieve path

//        dstar->updateStart(10,2);      // move start to (10,2)
        dstar->updateStart(curX,curY);
        dstar->replan();               // plan a path
        mypath = dstar->getPath();     // retrieve path
    /*
        dstar->updateGoal(0,1);        // move goal to (0,1)
        dstar->replan();               // plan a path
        mypath = dstar->getPath();     // retrieve path
       */
        int angle;
        int stepX;
        int stepY;
        vector<int>arrayX;
        vector<int>arrayY;
//        cout<<"total step is : "<<mypath.size()<<endl;
        //cout<<"start coordinates is "<<"("<<s_start.x<<","<<s_start.y<<")"<<endl;
        //cout<<"goal coordinates is "<<"("<<s_goal.x<<","<<s_goal.y<<")"<<endl;

        for (list<state>::iterator it = mypath.begin(); it != mypath.end(); it++)
        {
          stepX = it->x;
          stepY = it->y;
          arrayX.push_back(stepX);
          arrayY.push_back(stepY);
//          cout<<"("<<stepX<<","<<stepY<<")"<<endl;
        }

        for (int i=0; i<arrayX.size()-1;i++)
        {
          if (arrayX[i+1]-arrayX[i]>0 && arrayY[i+1]-arrayY[i]==0){angle = 0;}
          else if (arrayX[i+1]-arrayX[i]>0 && arrayY[i+1]-arrayY[i]>0){angle = 45;}
          else if (arrayX[i+1]-arrayX[i]==0 && arrayY[i+1]-arrayY[i]>0){angle = 90;}
          else if (arrayX[i+1]-arrayX[i]<0 && arrayY[i+1]-arrayY[i]>0){angle = 135;}
          else if (arrayX[i+1]-arrayX[i]<0 && arrayY[i+1]-arrayY[i]==0){angle = 180;}
          else if (arrayX[i+1]-arrayX[i]<0 && arrayY[i+1]-arrayY[i]<0){angle = 225;}
          else if (arrayX[i+1]-arrayX[i]==0 && arrayY[i+1]-arrayY[i]<0){angle = 270;}
          else if (arrayX[i+1]-arrayX[i]>0 && arrayY[i+1]-arrayY[i]<0){angle = 315;}
          planningAngle.push_back(angle);
//          cout<<"Planning angle is "<<angle<<endl;
        }

    }
    //path end
    static void detectedhuman()
    {
        //initial

        if(is_file_exist("result/result_3d_1.txt") == true)
        {
            std::cout<<"initial located"<<"\n"<<std::flush;
            std::ifstream infile("result/result_3d_1.txt");
            std::string line;
            while (std::getline(infile, line))
            {
                std::istringstream iss(line);
                if (!(iss >> objID >> targetX >> targetY >> targetZ))
                {
                    std::cout<<"empty file"<<"\n"<<std::flush;
                } // error
            }
            previousID = objID;
            previousX = targetX;
            previousY = targetY;
            previousZ = targetZ;
            isHuman=true;
        }
        else
        {
            std::cout<<"no human"<<"\n"<<std::flush;
            isHuman=false;
        }

    }
    //------------
    bool checkDirection()
    {
        read2files();
        float angle = location[2];
        std::cout<<location[2]<<"\n";

        cv::Mat originalfirst = (cv::Mat_<double>(2,1) << firstX,firstZ);
        std::cout<<"originalfirst is "<<originalfirst<<"\n";

        cv::Mat trans = (cv::Mat_<double>(2,1) << location[0],location[1]);
        std::cout<<"trans is "<<trans<<"\n";

        cv::Mat transAngle = (cv::Mat_<double>(2,2) << cos(angle),-sin(angle),sin(angle),cos(angle));
        std::cout<<"transAngle is "<<transAngle<<"\n";

        cv::Mat transformedfirst = transAngle*originalfirst + trans;
        std::cout<<"transformedfirst is "<<transformedfirst<<"\n";

        cv::Mat originalsecond = (cv::Mat_<double>(2,1) << secondX,secondZ);
        std::cout<<"originalsecond is "<<originalsecond<<"\n";

        cv::Mat transformedsecond = transAngle*originalsecond + trans;
        std::cout<<"transformedsecond is "<<transformedsecond<<"\n";

        vector<float> vec1;
        vector<float> vec2;

        transformedfirst.col(0).copyTo(vec1);
        transformedsecond.col(0).copyTo(vec2);

//        std::cout<<"vec1 is "<<vec1<<"\n";
//        std::cout<<"vec2 is "<<vec2<<"\n";
        humanAngle = (atan2((vec2[1]-vec1[1]),(vec2[0]-vec1[0]))*180/CV_PI);
        std::cout<<"humanAngle is"<<humanAngle<<"\n"<<std::flush;
        if(humanAngle<0)
        {
            humanAngle = humanAngle+360;
        }

        std::cout<<"converted humanAngle is"<<humanAngle<<"\n"<<std::flush;
        std::cout<<"planningAngle is "<<planningAngle[0]<<"\n"<<std::flush;

        float angleThresh = 20;
        if(abs(planningAngle[0] - humanAngle) < angleThresh)
        {
            std::cout<<"same direction"<<"\n";
            correctDirection=true;
        }
        else
        {
            std::cout<<"not same direction"<<"\n";
            correctDirection=false;
        }
        originalfirst.release();
        trans.release();
        transAngle.release();
        transformedfirst.release();
        originalsecond.release();
        transformedsecond.release();
    }
//----------------------------
    static bool read2files()
    {
        string firstresult,secondresult;
        string frontname = "result/result_3d_";
        string backname = ".txt";
        char numstr1[21],numstr2[21];

        sprintf(numstr1, "%d", i);
        sprintf(numstr2, "%d",i+1);
        firstresult = frontname + numstr1 + backname;
        std::cout<<firstresult<<"\n";
        if(is_file_exist(firstresult.c_str()) == true)
        {
            nofile = false;
            std::ifstream infilefirst(firstresult.c_str());
            std::string linefirst;
            while (std::getline(infilefirst, linefirst))
            {
                std::istringstream issfirst(linefirst);
                if (!(issfirst >> firstID >> firstX >> firstY >> firstZ))
                {
                    std::cout<<"first file empty"<<"\n"<<std::flush;
                    break;
                }

            }
            showCenters(firstID, firstX, firstY, firstZ);
        }
        else
        {
            std::cout<<"no first file"<<"\n";
            nofile = true;
        }

        secondresult = frontname + numstr2 + backname;
        std::cout<<secondresult<<"\n";
        if(is_file_exist(secondresult.c_str()) == true)
        {
            nofile = false;
            std::ifstream infilesecond(secondresult.c_str());
            std::string linesecond;
            while (std::getline(infilesecond, linesecond))
            {
                std::istringstream isssecond(linesecond);
                if (!(isssecond >> secondID >> secondX >> secondY >> secondZ))
                {
                    std::cout<<"second file empty"<<"\n"<<std::flush;
                    break;
                }
            }
            showCenters(secondID, secondX, secondY, secondZ);
        }
        else
        {
            std::cout<<"no second file"<<"\n";
            nofile = true;
        }
    }

    static bool is_file_exist(const char *fileName)
    {
        std::ifstream ifile(fileName);
        return (bool)ifile;
    //  return (bool)std::ifstream(fileName);
    }

    static void showCenters(float id, float x, float y, float z)
    {
        std::cout<<"id is "<<id<<"\n"<<std::flush;
        std::cout<<"x is "<<x<<"\n"<<std::flush;
        std::cout<<"y is "<<y<<"\n"<<std::flush;
        std::cout<<"z is "<<z<<"\n"<<std::flush;
    }
    void Draw(cv::Mat &canvas, double propertyThres=0.1)
    {
        ToST tst;

        tst.Initial();

        std::ostringstream oss;
        oss<<property;
        if(property>propertyThres)
        {
            cv::rectangle(canvas, roiDigit[0], cv::Scalar(0,0,255), 5);
            cv::rectangle(canvas, roiDigit[1], cv::Scalar(0,255,0), 5);
            oss<<"  "<<location;

            //add path finding
//----------------------------------------------------------------------------------------
            endX = 0;
            endY = 2950;
            //point 10 (0,650)
            float distancethresh = 30;
            initX = location[0];
            initY = location[1];
            planningAngle.clear();
            pathFinding(initX,initY,endX,endY);
            detectedhuman();
            string result;
            string frontname = "result/result_3d_";
            string backname = ".txt";
            char numstr[21];
            while(isHuman==true)
            {
                //-------------------
                checkDirection();
                if(nofile==true)
                {
                    std::cout<<"dont follow"<<"\n";
                    break;
                }
                if(correctDirection==false)
                {
                    std::cout<<"wrong direction"<<"\n";
                    break;
                }
                else
                {
                    while(correctDirection==true)
                    {
                       std::cout<<"followed"<<"\n";
                       sprintf(numstr, "%d", i);
                       result = frontname + numstr + backname;
                       if(is_file_exist(result.c_str()) == true)
                       {
                           std::cout<<"i is "<<i;
                           //clean count missed frames
                           cntmissed=0;
                           std::cout<<"located"<<"\n"<<std::flush;
                           std::ifstream infile(result.c_str());
                           std::string line;
                           while (std::getline(infile, line))
                           {
                               std::istringstream iss(line);
                               if (!(iss >> objID >> targetX >> targetY >> targetZ))
                               {
                                   std::cout<<"empty file"<<"\n"<<std::flush;
                                   break;
                               } // error
                           }
                           currentID = objID;
                           showCenters(objID, targetX, targetY, targetZ);

                           //fixed speed
                           float speedX = 0.1;
                           float speedY = 180;
                           //distance gap
                           float dangerGap = 0.5;
                           float right_left_gap = 0.05;

                           //stop if too close for any person showed up
                           if(targetZ < 0.3)
                           {
                               std::cout<<"about to collide"<<"\n"<<std::flush;
                               tst.Move();
                               break;
                           }

                           //different person situation
                           if (previousID != currentID)
                           {
                               std::cout<<"different person"<<"\n"<<std::flush;
                               cntfalse++;
                               if(cntfalse<10)
                               {
                                 std::cout<<"this is another person"<<"\n"<<std::flush;
                                 //check if path finding or track another person
                                 tst.Move();
                                 break;
                               }
                               if (previousZ - dangerGap > 0)
                               {
                                   if (previousX > right_left_gap)
                                   {
                                       tst.Move(0,speedY,-speedX);
                                       usleep(100000);
                                   }
                                   else if (previousX < -(right_left_gap))
                                   {
                                       tst.Move(0,speedY,speedX);
                                       usleep(100000);
                                   }
                                   else
                                   {
                                       tst.Move(0,speedY,0);
                                       usleep(100000);
                                   }

                               }
                               else
                               {
                                   std::cout<<"too close, danger!"<<"\n"<<std::flush;
                                   tst.Move();
                                   break;
                               }

                           }

                           //follow the same person situation
                           else if (previousID == currentID)
                           {
                               cntfalse=0;
                               std::cout<<"same person"<<"\n"<<std::flush;
                               previousX = targetX;
                               previousY = targetY;
                               previousZ = targetZ;
                               if (targetZ - dangerGap > 0)
                               {
                                   std::cout<<"im here";
                                   if (targetX > right_left_gap)
                                   {
                                       tst.Move(0,speedY,-speedX);
                                       usleep(100000);
                                   }
                                   else if (targetX < -(right_left_gap))
                                   {
                                       tst.Move(0,speedY,speedX);
                                       usleep(100000);
                                   }
                                   else
                                   {
                                       tst.Move(0,speedY,0);
                                       usleep(100000);
                                   }
                               }
                               else
                               {
                                   std::cout<<"too close, danger!"<<"\n"<<std::flush;
                                   tst.Move();
                                   break;
                               }

                           }
                           i++;
                       }
                       else
                       {
                           cntmissed++;
                           std::cout<<"missing frame, waiting..."<<"\n"<<std::flush;
                           usleep(100000);
                           if(cntmissed > 60)
                           {
                               std::cout<<"missing toooooo many, stop!"<<"\n"<<std::flush;
                               //check if path finding or track another person
                               tst.Move();
                               break;
                           }
                       }
                    }
//                    else{break;}
                }
            }
            if(abs(initX-endX)<distancethresh && abs(initY-endY)<distancethresh)
            {
                std::cout<<"reached"<<"\n"<<std::flush;
                tst.Move();
                k0=27;
            }
            else
            {
                if(planningAngle[0]==0)
                {
                    std::cout<<"angle is 0"<<"\n"<<std::flush;
                    tst.Move(-120,0,0);
                }
                else if(planningAngle[0]==45)
                {
                    std::cout<<"angle is 45"<<"\n"<<std::flush;
                    tst.Move(-120,-120,0);
                }
                else if(planningAngle[0]==90)
                {
                    std::cout<<"angle is 90"<<"\n"<<std::flush;
                    tst.Move(0,-120,0);
                }
                else if(planningAngle[0]==135)
                {
                    std::cout<<"angle is 135"<<"\n"<<std::flush;
                    tst.Move(120,-120,0);
                }
                else if(planningAngle[0]==180)
                {
                    std::cout<<"angle is 180"<<"\n"<<std::flush;
                    tst.Move(120,0,0);
                }
                else if(planningAngle[0]==225)
                {
                    std::cout<<"angle is 225"<<"\n"<<std::flush;
                    tst.Move(120,120,0);
                }
                else if(planningAngle[0]==270)
                {
                    std::cout<<"angle is 270"<<"\n"<<std::flush;
                    tst.Move(0,120,0);
                }
                else if(planningAngle[0]==315)
                {
                    std::cout<<"angle is 315"<<"\n"<<std::flush;
                    tst.Move(-120,120,0);
                }
            }
        }


        else
        {
            std::cout<<"no target stop !"<<"\n";
            tst.Move(0,0,-0.1);
        }
//------------------------------------------------------------------------
        std::string res=oss.str();
        cv::putText(canvas, res, cv::Point(0,canvas.rows-1),cv::FONT_HERSHEY_SIMPLEX,2.6,cv::Scalar(0,0,255),2);
    }

};

#endif // ANALYSISRESULT_HPP

